#include <math.h>

#include <QString>
#include <QtTest>

#include "MassSpringDamper.h"

#include <math/MSc_IntegratorRungeKutta4.h>

////////////////////////////////////////////////////////////////////////////////

class MSc_IntegratorRungeKutta4Test : public QObject
{
    Q_OBJECT

public:
    MSc_IntegratorRungeKutta4Test();

private Q_SLOTS:
    void testIntegrate();
};

////////////////////////////////////////////////////////////////////////////////

MSc_IntegratorRungeKutta4Test::MSc_IntegratorRungeKutta4Test() {}

////////////////////////////////////////////////////////////////////////////////

void MSc_IntegratorRungeKutta4Test::testIntegrate()
{
    // initial value
    const double s_0[ 2 ] = { 1.0, 0.0 };

    double s_1[ 2 ];
    double s_2[ 2 ];

    s_1[ 0 ] = s_0[ 0 ];
    s_1[ 1 ] = s_0[ 1 ];

    MassSpringDamper msd;

    MSc_IntegratorRungeKutta4< MassSpringDamper >
            integrator( 2, &msd, &MassSpringDamper::computeDeriv );

    const double dt = 0.01; // 0.01s time step
    const double tf = 10.0; // 10s

    int steps = tf / dt;    // number of steps (for 60s. period)

    // numerical integration
    for ( int i = 0; i < steps; i++ )
    {
        integrator.integrate( dt, s_1 );
    }

    // analytical solution
    msd.computeState( s_0, s_2, tf );

    double delta = pow( s_1[ 0 ] - s_2[ 0 ], 2.0 )
                 + pow( s_1[ 1 ] - s_2[ 1 ], 2.0 );

    QVERIFY( ( delta < 10.0e-4 ) );
}

////////////////////////////////////////////////////////////////////////////////

QTEST_APPLESS_MAIN(MSc_IntegratorRungeKutta4Test)

////////////////////////////////////////////////////////////////////////////////

#include "tst_msc_integratorrungekutta4test.moc"
